#include <Arduino.h>
#include "SPI.h"
#include "TFT_eSPI.h"

void interruptServiceRoutine(void);
void gottouch1(void);
void gottouch2(void);
void gottouch3(void);
void gottouch4(void);
void setdir_speed(bool dir, uint32_t speed);
void setdir_speed2(bool dir, uint32_t speed);
void PWM_init();
void Touch_init();


/***屏幕宏定义***/
// #define TFT_MISO -1
// #define TFT_MOSI 33
// #define TFT_SCLK 32
// #define TFT_CS   26  // Chip select control pin
// #define TFT_DC   25  // Data Command control pin
// #define TFT_RST   4  // Reset pin (could connect to RST pin)
// #define TFT_RST  -1  // Set TFT_RST to -1 if display RESET is connected to ESP32 board RST

/***微动开关定义***/
#define SW1 35
#define SW2 34

/***触摸宏定义***/
#define thes  13
#define threshold1 thes
#define threshold2 thes
#define threshold3 thes
#define threshold4 thes
#define TOUCH1 T4 //13
#define TOUCH2 T5 //12
#define TOUCH3 T6 //14
#define TOUCH4 T7 //27
#define TOUCH_TIME 500

/***电机宏定义***/
#define motor_PWM_frq 10000
#define motor_PWM_res 8
#define motor_PWMA 22
#define motor_PWM_channelA 1
#define motor_PWMB 23
#define motor_PWM_channelB 2
#define motor_PWMC 21
#define motor_PWM_channelC 3
#define motor_PWMD 19
#define motor_PWM_channelD 4

int pwm_vaule = 120;
int pwm_vaule2 = 80;
int big_motor_speed = 0;
int small_motor_speed = 0;

/***称重模块***/
#define HX_SCK 5
#define HX_DT 36
#define HX_SCK2 18
#define HX_DT2 39
long count0 = 0;
long count = 0;

/***串口***/
char a;
int b;

TFT_eSPI tft = TFT_eSPI();
void setup()
{
    delay(100);
    Serial.begin(115200);
    Serial.print("Serial Begin!");
    PWM_init();
    Touch_init();
    HX_init();
    delay(100);
    setdir_speed(true, 0);
    setdir_speed2(true, 0);

    /***************屏幕**************/
    Serial.println("TFT_init!");
    tft.begin();
    tft.setRotation(3); // landscape
    tft.fillScreen(TFT_BLACK);
    Serial.println("TFT_init......");
    // tft.fillRect(0, 0, 320, 14, TFT_RED);
    //drawNumber(long long_num, int32_t poX, int32_t poY) //draw a long integer
}

void loop()
{
    tft.drawString("Motor1Speed:", 0, 0, 2);
    tft.drawString("Motor2Speed:", 0, 16, 2);
    tft.drawString("    ", 88, 16, 2);
    tft.drawNumber(small_motor_speed, 88, 16, 2);
    tft.drawString("    ", 88, 0, 2);
    tft.drawNumber(big_motor_speed, 88, 0, 2);
    tft.drawString("HX711:", 0, 32, 2);
    while (true)
    {
        count = ReadCount() - count0;
        Serial.print("Count0=");
        Serial.print(count0);
        Serial.print("     ");
        Serial.print("Count=");
        Serial.println(count);
        tft.drawString("              ",48,32,2);
        tft.drawNumber(count, 48, 32, 2);
        // tft.fillScreen(TFT_BLACK);
        delay(500);
    }
}

void HX_init()
{
    pinMode(HX_SCK, OUTPUT);
    pinMode(HX_DT, INPUT);
    ReadCount();
    delay(100);
    for (int i = 0; i < 8; i++)
        count0 += ReadCount();
    count0 /= 8;
}

long ReadCount()
{
    pinMode(HX_SCK, OUTPUT);
    pinMode(HX_DT, INPUT);
    digitalWrite(HX_SCK, LOW);
    while (digitalRead(HX_DT))
        ;
    unsigned long count_temp = 0;
    for (int i = 0; i < 24; i++)
    {
        digitalWrite(HX_SCK, HIGH);
        count_temp <<= 1;
        digitalWrite(HX_SCK, LOW);
        if (digitalRead(HX_DT))
            count_temp |= 1;
    }
    digitalWrite(HX_SCK, HIGH);
    if (count_temp & 0x00800000)
        count_temp |= 0xFF000000;
    digitalWrite(HX_SCK, LOW);
    return (long)count_temp;
}

void Touch_init()
{
    touchAttachInterrupt(TOUCH1, gottouch1, threshold1);
    touchAttachInterrupt(TOUCH2, gottouch2, threshold2);
    touchAttachInterrupt(TOUCH3, gottouch3, threshold3);
    touchAttachInterrupt(TOUCH4, gottouch4, threshold4);
}

void PWM_init()
{

    ledcAttachPin(motor_PWMA, motor_PWM_channelA);
    ledcSetup(motor_PWM_channelA, motor_PWM_frq, motor_PWM_res);
    ledcWrite(motor_PWM_channelA, 0);
    ledcAttachPin(motor_PWMB, motor_PWM_channelB);
    ledcSetup(motor_PWM_channelB, motor_PWM_frq, motor_PWM_res);
    ledcWrite(motor_PWM_channelB, 0);

    ledcAttachPin(motor_PWMC, motor_PWM_channelC);
    ledcSetup(motor_PWM_channelC, motor_PWM_frq, motor_PWM_res);
    ledcWrite(motor_PWM_channelC, 0);
    ledcAttachPin(motor_PWMD, motor_PWM_channelD);
    ledcSetup(motor_PWM_channelD, motor_PWM_frq, motor_PWM_res);
    ledcWrite(motor_PWM_channelD, 0);
}

void setdir_speed(bool dir, uint32_t speed)
{
    static bool lastdir = true;
    static uint8_t chl = motor_PWM_channelA;
    if (speed != 0)
        speed = speed * ((255.0 - pwm_vaule) / 100.0) + pwm_vaule;
    // Serial.println(speed);
    if (lastdir == dir)
    {
        ledcWrite(chl, speed);
    }
    else
    {
        lastdir = dir;
        ledcWrite(chl, 0);
        if (dir)
            chl = motor_PWM_channelA;
        else
            chl = motor_PWM_channelB;
        ledcWrite(chl, speed);
    }
    // Serial.println(ledcRead(0));
    // Serial.println(ledcRead(1));
}

void setdir_speed2(bool dir, uint32_t speed)
{
    static bool lastdir = true;
    static uint8_t chl = motor_PWM_channelC;
    if (speed != 0)
        speed = speed * ((255.0 - pwm_vaule2) / 100.0) + pwm_vaule2;
    // Serial.println(speed);
    if (lastdir == dir)
    {
        ledcWrite(chl, speed);
    }
    else
    {
        lastdir = dir;
        ledcWrite(chl, 0);
        if (dir)
            chl = motor_PWM_channelC;
        else
            chl = motor_PWM_channelD;
        ledcWrite(chl, speed);
    }
    // Serial.println(ledcRead(2));
    // Serial.println(ledcRead(3));
}

// void interruptServiceRoutine(void)
// {
//     Serial.print("IRP!");
// }

void gottouch1(void)
{
    delay(TOUCH_TIME);
    // touchAttachInterrupt(TOUCH1, gottouch1, 0);
    if (touchRead(TOUCH1) < threshold1)
    {
        if (big_motor_speed < 100)
            big_motor_speed++;
        else
            big_motor_speed = 100;
        if (big_motor_speed > 0)
            setdir_speed(true, big_motor_speed);
        else
            setdir_speed(false, -big_motor_speed);
        tft.drawString("      ", 88, 0, 2);
        tft.drawNumber(big_motor_speed, 88, 0, 2);
        Serial.print("Touch1:");
        Serial.println(touchRead(TOUCH1));
    }
    // touchAttachInterrupt(TOUCH1, gottouch1, threshold1);
}

void gottouch2(void)
{
    delay(TOUCH_TIME);
    if (touchRead(TOUCH2) < threshold2)
    {
        if (big_motor_speed > -100)
            big_motor_speed--;
        else
            big_motor_speed = -100;
        if (big_motor_speed > 0)
            setdir_speed(true, big_motor_speed);
        else
            setdir_speed(false, -big_motor_speed);
        tft.drawString("      ", 88, 0, 2);
        tft.drawNumber(big_motor_speed, 88, 0, 2);
        Serial.print("Touch2:");
        Serial.println(touchRead(TOUCH2));
    }   
}

void gottouch3(void)
{
    delay(TOUCH_TIME);
    if (touchRead(TOUCH3) < threshold3)
    {
        if (small_motor_speed < 100)
            small_motor_speed++;
        else
            small_motor_speed = 100;
        if (small_motor_speed > 0)
            setdir_speed2(true, small_motor_speed);
        else
            setdir_speed2(false, -small_motor_speed);
        tft.drawString("      ", 88, 16, 2);
        tft.drawNumber(small_motor_speed, 88, 16, 2);
        Serial.print("Touch3:");
        Serial.println(touchRead(TOUCH3));
    }
}

void gottouch4(void)
{
    delay(TOUCH_TIME);
    if (touchRead(TOUCH4) < threshold4)
    {
        if (small_motor_speed > -100)
            small_motor_speed--;
        else
            small_motor_speed = -100;
        if (small_motor_speed > 0)
            setdir_speed2(true, small_motor_speed);
        else
            setdir_speed2(false, -small_motor_speed);
        tft.drawString("      ", 88, 16, 2);
        tft.drawNumber(small_motor_speed, 88, 16, 2);
        Serial.print("Touch4:");
        Serial.println(touchRead(TOUCH4));
    }
}

/**

// ESP32 Touch Test
// Just test touch pin - Touch0 is T0 which is on GPIO 4.

void setup()
{
  Serial.begin(115200);
  delay(1000); // give me time to bring up serial monitor
  Serial.println("ESP32 Touch Test");
}

void loop()
{
  Serial.println(touchRead(T0));  // get value using T0
  delay(1000);
}


    // pinMode(SW1,INPUT);
    // attachInterrupt(SW1,interruptServiceRoutine,RISING);
    //T1 T2有其他用图  一般不能用于touch
    // Serial.print("T1:");
    // Serial.print(touchRead(T0));
    // Serial.print("   T2:");
    // Serial.print(touchRead(T3));
    // Serial.print("   T3:");
    // Serial.print(touchRead(T6));
    // Serial.print("   T4:");
    // Serial.println(touchRead(T7));

    // touchAttachInterrupt(T1,gottouch1,threshold1);
    // touchAttachInterrupt(T2,gottouch2,threshold2);
    // attachInterrupt(SS,interruptServiceRoutine,FALLING);
    // ledcDetachPin(PWM_Pin);  //这个是解除IO口的pwm输出功能模式
*/

/***********HX711***************/
/*
uint8_t HX_SCK = D3;
uint8_t HX_DT = D4;
long count0;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(HX_SCK, OUTPUT);
  pinMode(HX_DT, INPUT);
  ReadCount();
  delay(100);
  count0 = 0;
  for(int i=0; i<8; i++)
    count0 += ReadCount();
  count0 /= 8;
}

void loop() {
  // put your main code here, to run repeatedly:
  long count = ReadCount() - count0;
  Serial.println(count);
  delay(100);
}

long ReadCount()
{
  digitalWrite(HX_SCK, LOW);
  while(digitalRead(HX_DT));
  unsigned long count = 0;
  for(int i=0; i<24; i++)
  {
    digitalWrite(HX_SCK, HIGH);
    count <<= 1;
    digitalWrite(HX_SCK, LOW);
    if(digitalRead(HX_DT)) count |= 1;
  }
  digitalWrite(HX_SCK, HIGH);
  if(count & 0x00800000) count |= 0xFF000000;
  digitalWrite(HX_SCK, LOW);
  return (long)count;
}
*/